#include "main.h"
#include "Speed_Control.h"
#include "encoder.h"
Speed_Execution Motor;

int speed_flag = 0;

float Function_Frequency = 0.0001f;

void Profile_Generator_Init(Speed_Execution* motor,float Distance,float Accelerate,float Velocity_Max)
{
    motor->Direction = 1;

    motor->Current_Velocity = 0;

    motor->Current_Time = 0;

    motor->Current_Position = Encoder_Data_Real.Now;

    if(Distance < 0)
    {
      motor->Direction = -1;
    }

    motor->Distance = Distance;

    motor->Accelerate = Accelerate * motor->Direction;

    motor->Velocity_Max = Velocity_Max * motor->Direction;

    motor->T_acc = motor->Velocity_Max / motor->Accelerate;

    motor->D_acc = 0.5f * motor->Accelerate * motor->T_acc * motor->T_acc;

    motor->T_full = (motor->Distance - 2 * motor->D_acc) / motor->Velocity_Max;
}

void Profile_Generator_Caculate(Speed_Execution *motor)
{
  //非理想情况
  if(motor->Accelerate * motor->T_acc * motor->T_acc * motor->Direction >= motor->Distance * motor->Direction)
  {

    motor->T_acc = sqrtf(motor->Distance/motor->Accelerate);
    if(motor->Current_Time <= motor->T_acc)
    {
      motor->Current_Accelerate = motor->Accelerate;

      motor->Current_Velocity = motor->Current_Velocity + motor->Accelerate * Function_Frequency;

      motor->Current_Position = motor->Current_Position + motor->Current_Velocity * Function_Frequency
              + 0.5f * motor->Accelerate * Function_Frequency * Function_Frequency;

      motor->Current_Time += Function_Frequency;
    }
    else if(motor->Current_Time <= 2*motor->T_acc)
    {
      motor->Current_Accelerate = -motor->Accelerate;

      motor->Current_Velocity = motor->Current_Velocity - motor->Accelerate * Function_Frequency;

      motor->Current_Position = motor->Current_Position + motor->Current_Velocity * Function_Frequency
              - 0.5f * motor->Accelerate * Function_Frequency * Function_Frequency;

      motor->Current_Time += Function_Frequency;
    }
    else
    {
      motor->Current_Accelerate = 0;
      motor->Current_Velocity = 0;
    }
  }
  //理想情况下
  else
  {
    //速度上升
    if(motor->Current_Time <= motor->T_acc)
    {
      motor->Current_Accelerate = motor->Accelerate;

      motor->Current_Velocity = motor->Current_Velocity + motor->Accelerate * Function_Frequency;

      motor->Current_Position = motor->Current_Position + motor->Current_Velocity * Function_Frequency
              + 0.5f * motor->Accelerate * Function_Frequency * Function_Frequency;

      motor->Current_Time += Function_Frequency;
    }
    //速度保持
    else if(motor->Current_Time <= motor->T_acc + motor->T_full)
    {
      motor->Current_Accelerate = 0;

      motor->Current_Position = motor->Current_Position + motor->Current_Velocity * Function_Frequency;

      motor->Current_Time += Function_Frequency;
    }
    //速度下降
    else if(motor->Current_Time <= 2 * motor->T_acc + motor->T_full )
    {
      motor->Current_Accelerate = -motor->Accelerate;

      motor->Current_Velocity = motor->Current_Velocity - motor->Accelerate * Function_Frequency;

      motor->Current_Position = motor->Current_Position + motor->Current_Velocity * Function_Frequency - 0.5f * motor->Accelerate * Function_Frequency * Function_Frequency;

      motor->Current_Time += Function_Frequency;
    }
    else
    {
      motor->Current_Accelerate = 0;
      motor->Current_Velocity = 0;
    }
  }
}

